module Colad_module
   # check for static collision, static collisions are collisions which sensors can't
  # detect
  def static_collision
      oldins=@ins
      self.recieve
      sleep(0.5)
      stat_movdis=Math.sqrt( (oldins[0].to_f-@ins[0].to_f).abs*(oldins[0].to_f-@ins[0].to_f).abs+(oldins[1].to_f-@ins[1].to_f).abs*(oldins[1].to_f-@ins[1].to_f).abs+(oldins[2].to_f-@ins[2].to_f).abs*(oldins[2].to_f-@ins[2].to_f).abs  ) 
      #puts stat_movdis
      if(@speed<1.1) #when the robot is moving very slow, check for collitions at a very small length
        check=0.01
      else
        check=0.01*( (@speed*2) )
      end
    if( stat_movdis > check )
       puts "false"
      @static_col=0
     else
       puts "true"
       #break
       @connect.write("DRIVE {Left 0.0} {Right 0.0} \r\n")
       @stop=1 #order to stop
       @static_col=1 #used for static collision checks
       @moveflag="" #clear move flag
       sleep(0.5) #wait for the collision flag to show itself
      end
   end


  def colguardmode   
   Thread.new{
    Thread.current["name"]="r_#{@robotname}_staticcol"
    puts "collision guard mode on"
    @colguardmode=1
    while (@colguardmode==1)
      #@static_col=0 if (@moveflag=="")
      self.static_collision if(@moveflag=="forward" or @moveflag=="backward")
    sleep(0.05)
    end
   }
  end
  
 
  
  #Checks to see if there is an obsticle in front of the robot
  #currently checks by using the robots 4th and 5th front sensor
  #returns true if robot is about to collide
  def col_frontcheck
    
    if (@fSONARSEN[3].to_f < 0.5 or @fSONARSEN[4].to_f < 0.5 )
      return true
    else
      return false
    end
    
    #check for static collisions
    
    
  end
  
  #Checks to see if there is an obsticle in front of the robot
  #currently checks by using the robots 4th and 5th rear sensor
  #returns true if robot is about to collide
  def col_rearcheck
    if (@rSONARSEN[3].to_f < 0.5 or @rSONARSEN[4].to_f < 0.5 )
      return true
    else
      return false
    end
  end
  
  #Checks to see if the robot can continue to the left side
  #currently checks by using the robots 1st and 2nd front sensor
  def left_free    
   if( (@fSONARSEN[0].to_f+ @fSONARSEN[1].to_f)/2 > 0.5)
    return true
   else
    return false
   end
  end
  
  #Checks to see if the robot can continue to the right side
  #currently checks by using the robots 7th and 8th front sensor
  def right_free
   if( (@fSONARSEN[6].to_f+ @fSONARSEN[7].to_f)/2 > 0.5)
    return true
   else
    return false
   end
  end  
end